/* 
***目前有NLMS算法
***LMS法，为可观性，用了+*运算符重载，并返回的值而不是引用，向量加和向量数乘运算可以合并
***部分测试需要的还未删除

对外接口：对外接口：_Creat(),_Cal()

 */

#include <iostream>
#include <tuple>
#include <utility>
#include <cmath>
#include <algorithm>
#include <vector>
#include <functional>

#include "../include/LMS.h"

#define π 3.141592653589793238462643383279


//复位
static void renew( LMS_Struct & lms , int N){
    lms.N = N;
    //lms.dn = 0;
    lms.yn = 0;
    lms.en = 0;
    lms.tr_R = 0;

    lms.xn.clear(); //清空向量中所有元素
    lms.w.clear(); //清空向量中所有元素    
    //lms.xn.resize( N , 0 ); //将现有元素个数调至N个，多则删，少则补，其值0
    lms.w.resize( N , 0 ); //将现有元素个数调至N个，多则删，少则补，其值0
}

//向量内积
template < typename T >
static T dot( std::vector<T> const & x1 , std::vector<T> const & x2 ) 
{
    T d = 0;
    auto N = std::min( x1.size() , x2.size() );
    for( int i=0 ; i < N ; i++ ){
        d += x1[i]*x2[i];
    }     
    return d ;
}

//向量×标量，y=cx
template < typename T >
static std::vector<T>  operator*( T c , std::vector<T> const & x ) 
{
    /* for(vector<T>::iterator it=x.begin();it!=x1.end();it++){
        *it = (*it) *c;
    } */
    std::vector<T> y(x); //用x向量来创建y向量，整体复制性赋值    
    for (int i = 0; i < y.size(); i++){
        y[i] = x[i]*c;
    }
    return y;
}

//向量+向量，y=x+b ； b为梯度向量，x宽度修改为跟随b（后续补零）
template < typename T >
static std::vector<T> operator+ ( std::vector<T> & x , std::vector<T> const & b )
{
    std::vector<T> y(b); //用x向量来创建y向量，整体复制性赋值
    if(  x.size() < b.size() ) [[unlikely]] {
        x.resize( b.size() , (T)0 ); //将现有元素个数调至N个，多则删，少则补，其值为0
    }

    for(int i=0 ; i < y.size() ; i++ ){
        y[i] = x[i] + b[i];
    }

    return y;
} 

//输入向量自相关矩阵迹
template <typename T>
static auto 自相关迹(T tr_last, T x_last, T x_new /*, std::vector<T> const & x*/, bool 覆盖)
{
    T tr = 0;
    if (覆盖) [[likely]]
    {
        tr = tr_last - x_last * x_last + x_new * x_new;
    }
    else
    {
        // x(n) 未满
        tr = tr_last + x_new * x_new;
    }
    return tr;
}

//基于箕舌线的变步长
template < typename T >
static T 箕舌线变步长( T err , T a )
{
    auto miu = a * (1 - 1.0 / ( err*err + 1));    
    return miu;
}
// //基于箕舌线的变步长
// template < typename T >
// static T 箕舌线变步长( T err , T tr , T a )
// {
//     auto miu = a * (1 - 1.0 / ( err*err + 1));
//     auto miu_max = 2.0 / tr; // w(n) = αw(n−1) + μe(n) u(n) 对应 2 / tr(R)
//     miu = miu * miu_max;
//     // if( miu > miu_max ){
//     //     miu = miu_max;
//     // }  
//     return miu;
// }


//xn缓冲序列更新，最新元素在0
static auto xn更新( LMS_Struct & lms , floatx x )
{ 
    bool updata = false;
    lms.xn.insert( lms.xn.begin() ,x ); //在第0个插入x
    auto xlast = lms.xn.back();//最后一个值
    if( lms.xn.size() > lms.N  )  {        
        lms.xn.pop_back(); //去掉数组的最后一个数据
        updata =  true;
    }
    return std::make_tuple( xlast , updata );

}
// //xn缓冲序列更新，最新元素在最后方
// auto xn更新( LMS_Struct & lms , floatx x )
// {    
//     lms.xn.push_back( x ) ; //向量尾部增加一个元素
//     if( lms.xn.size() > lms.N ){
//         lms.xn.erase( lms.xn.begin() ); //删除向量中迭代器指向元素
//         return true;
//     }
//     return false;
// }


#ifdef 测试时单步显示
static void 单步显示(LMS_Struct const &l)
{
    for (auto i : l.xn)
        std::cout << " x:" << i << " ";
    std::cout << std::endl;
    std::cout << " μ:" << l.μ << std::endl;
    std::cout << " e:" << l.en << std::endl;
    for (auto i : l.w)
        std::cout << " w:" << i << " ";
    std::cout << std::endl;
    std::cout << " y:" << l.yn << std::endl;
    std::cout << std::endl;
}
#endif

// auto 输出 = [=](float *y, float *e , float *miu) {
//         //外部变量都隐式按值捕获
//         *y = l.yn;
//         *e = l.en;
//         *miu = l.μ; 
//     };
static void 输出(LMS_Struct const &l, floatx *y, floatx *e, floatx *miu)
{
    *y = l.yn;
    *e = l.en;
    *miu = l.μ;
}


// 创建结构体对象
LMS_Struct *LMS_Struct_Creat()
{
    try
    {
        auto p = new LMS_Struct{};
        return p;
    }
    catch (const std::bad_alloc &e)
    {
        return nullptr;
    }
}

// NLMS自适应滤波，包括FIR滤波器及w更新
void NLMS_Run(
    LMS_Struct *p,
    int reset,
    int N,
    floatx x,
    floatx d,
    floatx *y,
    floatx *e,
    floatx *miu)
{
    auto& l = *p;

    if( !reset ){ // reset为0时复位
        renew( l , N );
        输出( l , y, e , miu  );
        return;
    }else{
        l.w.resize( l.N , 0 ); //将现有元素个数调至N（reset时N才刷新）个，多则删，少则补，其值0
    }
    
    auto [ xlast , 是否覆盖 ] = xn更新( l , x ); //更新xn缓冲，xlast为最旧值
    l.tr_R = std::max( 自相关迹( l.tr_R , xlast , (floatx)x ,是否覆盖 ) , (floatx)1e-5 );    
    auto μ = 箕舌线变步长( l.en ,  (floatx)0.999 );//考虑到截断误差，不严格用精确数
    l.μ = μ / l.tr_R ; //μ' = μ/(xTx) , μ<2时迭代收敛

    // if ( ( l.xn.size() < 3 ) ) [[unlikely]]  {        
    //     return ; //滤波从x阶开始
    // }

    l.yn = dot( l.w , l.xn );//滤波器的输出
    l.dn = d ; //
    l.en = l.dn - l.yn; //第k次迭代的误差

    auto f_xeμ = ( l.μ * l.en )* l.xn; // f = μe(n)x(n) , x(n)为实数向量
    l.w  =  l.w + f_xeμ; //滤波器权值计算的迭代式


    输出( l , y, e , miu  );

#ifdef 测试时单步显示
    单步显示(LMS_Struct const &l)
#endif
}


// 给出w值； w为目标数组（指针），只修改w.size()宽度的值； N为w个数（从w0开始）；返回w向量当前宽度
int get_weight( const LMS_Struct *p , floatx*  pw , int N )
{
    int n = p->w.size();
    auto size = std::min( n , N );
    // auto Size = std::max( n , N );
    for ( int i = 0 ; i < size ; i++ ){
        pw[i] = p->w[i];
    }    

    // for ( int i = size ; i<Size ; i++ ){
    //     pw[i] = 0 ;
    // }
    return n ;

}


